/*
 * Router.cpp
 *
 *  Created on: Mar 13, 2011
 *      Author: kmueller
 */

#include "MapRouter.h"

#include "MapReader.h"
#include "Geo.h"
#include "Route.h"

#include<map>
#include<vector>
#include<set>
#include<iostream>

using namespace std;

namespace opennav {

RoutingSettings::RoutingSettings() {
	preferred = FASTEST;
	avoidToll = false;
	avoidHighways = false;
}

RoutingSettings::RoutingSettings(PreferredRoute pref, bool aToll, bool aHighways) {
	preferred = pref;
	avoidToll = aToll;
	avoidHighways = aHighways;
}

bool RoutingSettings::is_avoid_toll() const {
	return avoidToll;
}

bool RoutingSettings::is_avoid_highways() const {
	return avoidHighways;
}

RoutingSettings::PreferredRoute RoutingSettings::get_preferred_route() const {
	return preferred;
}

MapRouter::MapRouter(Map* map) {
	reader = map;

	RoutingSettings s(RoutingSettings::FASTEST, false, false);
	set_routing_settings(s);
}

void MapRouter::set_routing_settings(const RoutingSettings& s) {
	settings = s;
}

int MapRouter::find(const RoadJunction& src, const RoadJunction& dst, Route& route) {
	NodeInfo* source;
	source = new NodeInfo;
	source->junction = src;
	source->parent = NULL;
//	source->viaSegment
	source->G = 0;
//	source->H = 0;
	source->F = 0;

	return find(source, dst, route);
}

int MapRouter::find(NodeInfo* src, const RoadJunction& dst, Route& route) {
	multimap<int, NodeInfo*, comparator> open;
	set<int32_t> closed; // just saves junction offsets
	open.insert(pair<int, NodeInfo*> (src->F, src));

	while (!open.empty()) {
		multimap<int, NodeInfo*, comparator>::iterator begin = open.begin();
		const NodeInfo *x = static_cast<NodeInfo*> (begin->second);
		open.erase(begin);

		Coordinate a = x->junction.coordinate;
		if (a == dst.coordinate) {
			int length = 0;

			NodeInfo *cur = const_cast<NodeInfo*> (x);
			while (cur != src) {
				length += cur->viaSegment.length;

				RouteSegment segment(cur->parent->junction, cur->viaSegment,
						cur->junction);
				route.add_segment(segment);

				const NodeInfo* tmp = cur;
				cur = const_cast<NodeInfo*> (cur->parent);
				delete tmp;
			}

			return length;
		}
		closed.insert(x->junction.offset);

		vector<int32_t> &segments =
				const_cast<vector<int32_t>&> (x->junction.segment_offsets);

		for (vector<int32_t>::iterator it = segments.begin(); it
				!= segments.end(); it++) {
			RoadSegment segment;
			reader->get_segment(*it, segment);

			if (settings.is_avoid_toll() && segment.toll) {
				continue;
			}
			if (settings.is_avoid_highways() && segment.type == ROAD_Motorway) {
				continue;
			}

			RoadJunction y;
			if (segment.junction_from_offset == x->junction.offset) {
				if (segment.oneway && segment.backward) {
					continue;
				}
				reader->get_junction(segment.junction_to_offset, y);
			} else {
				if (segment.oneway && !segment.backward) {
					continue;
				}
				reader->get_junction(segment.junction_from_offset, y);
			}

			const set<int32_t>::iterator found = closed.find(y.offset);

			if (found == closed.end()) { // y is not in closed
				NodeInfo* info = new NodeInfo;
				info->junction = y;
				info->parent = x;
				info->viaSegment = segment;
				info->G = info->parent->G + assessment(segment);
//				info->H = heuristic(y, segment, dst);
				info->F = info->G + heuristic(y, segment, dst);

				const multimap<int, NodeInfo*, comparator>::iterator exists =
						findJunctionInList(open, info);

				if (exists == open.end()) {
					open.insert(pair<int, NodeInfo*> (info->F, info));
				} else {
					if (info->G < exists->second->G) {
						delete exists->second;
						open.erase(exists);
						open.insert(pair<int, NodeInfo*> (info->F, info));
					}
					else {
						delete info;
					}
				}
			}
		}
	}
	return -1;
}

multimap<int, MapRouter::NodeInfo*, MapRouter::comparator>::iterator MapRouter::findJunctionInList(
		multimap<int, NodeInfo*, comparator>& open, const NodeInfo* info) const {
	multimap<int, NodeInfo*, comparator>::iterator it;
	for (it = open.begin(); it != open.end(); it++) {
		Coordinate a = it->second->junction.coordinate;
		if (a == info->junction.coordinate)
			return it;
	}
	return it;
}

int MapRouter::assessment(const RoadSegment& segment) {
	switch (settings.get_preferred_route()) {
	case RoutingSettings::FASTEST: {
		int maxspeed = segment.get_max_speed();
		return (segment.length) / (maxspeed / 3.6);
	}
	case RoutingSettings::SHORTEST:
		return segment.length;
	}
	return segment.length;
}

int MapRouter::heuristic(const RoadJunction&x, const RoadSegment& segment,
		const RoadJunction& dst) {
	Coordinate a = x.coordinate;
	int distance = a.distanceTo(dst.coordinate);
	switch (settings.get_preferred_route()) {
		case RoutingSettings::FASTEST: {
			switch (segment.type) {
			case ROAD_Motorway:
//				distance *= 0.1;
				break;
			case ROAD_Type1:
				distance *= 3;
				break;
			case ROAD_Type2:
				distance *= 5;
				break;
			case ROAD_Type3:
				distance *= 7;
				break;
			case ROAD_Type4:
				distance *= 8;
				break;
			case ROAD_Type5:
				distance *= 10;
				break;
			case ROAD_Track:
				distance *= 15;
				break;
			default:
				break;
			}
			return distance;
		}
		case RoutingSettings::SHORTEST:
			return distance;
	}
	return distance;
}

}
